#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include "global.h"
#include "gcode.h"
#include "stepmotor.h"

#define STEPS_BUF_SIZE  8

typedef struct
{
    uint32_t steps[STEPS_MOTORS];
    uint32_t pscValue[STEPS_MOTORS];
    uint32_t arrValue[STEPS_MOTORS];
    uint32_t f[STEPS_MOTORS];
    uint8_t  dir[STEPS_MOTORS];
} LINE_DATA;

volatile LINE_DATA steps_buf[STEPS_BUF_SIZE];

volatile int8_t steps_buf_count;    // 指令缓冲队列个数，steps_buf

int8_t steps_buf_get, steps_buf_put;

LINE_DATA cur_steps_buf; // for debug only

uint8_t mx_enable = 0;

typedef struct mx_pin_init_s
{
    GPIO_TypeDef* Port;
    uint16_t        Pin;
} mx_pin_init_t;

typedef struct mx_timer_init_s
{
    TIM_TypeDef* Timer;
    uint8_t         IRQn;
    uint32_t        Clock;
    uint8_t         Channel;
} mx_timer_init_t;
/* Init Limit*/
const mx_pin_init_t mx_limits[] =
{
    { LIMIT_X_PORT, LIMIT_X_PIN },
    { LIMIT_Y_PORT, LIMIT_Y_PIN },
    { LIMIT_Z_PORT, LIMIT_Z_PIN },
    { NULL }
};

/* Init StepMotor enable ports & pins*/
const mx_pin_init_t mx_enables[] =
{
    { M0_EN_PORT, M0_EN_PIN },
    { M1_EN_PORT, M1_EN_PIN },
    { M2_EN_PORT, M2_EN_PIN },
    { M3_EN_PORT, M3_EN_PIN },
    { NULL }
};
/* Init StepMotor direction ports & pins*/
const mx_pin_init_t mx_dirs[] =
{
    { M0_DIR_PORT, M0_DIR_PIN },
    { M1_DIR_PORT, M1_DIR_PIN },
    { M2_DIR_PORT, M2_DIR_PIN },
    { M3_DIR_PORT, M3_DIR_PIN },
    { NULL }
};

/* Init StepMotor steps ports & pins*/
const mx_pin_init_t mx_steps[] =
{
    { M0_STEP_PORT, M0_STEP_PIN },
    { M1_STEP_PORT, M1_STEP_PIN },
    { M2_STEP_PORT, M2_STEP_PIN },
    { M3_STEP_PORT, M3_STEP_PIN },
    { NULL }
};

/* Init StepMotor timers */
const mx_timer_init_t mx_timers[] =
{
    { M0_TIM, M0_TIM_IRQn, M0_TIM_CLK, M0_TIM_CHN },
    { M1_TIM, M1_TIM_IRQn, M1_TIM_CLK, M1_TIM_CHN },
    { M2_TIM, M2_TIM_IRQn, M2_TIM_CLK, M2_TIM_CHN },
    { M3_TIM, M3_TIM_IRQn, M3_TIM_CLK, M3_TIM_CHN },
    { NULL }
};

volatile struct
{
    int32_t         globalSteps;    // 全局走的步数
    uint32_t        steps;
    bool            clk;
    bool            dir;
    volatile bool   isInProc;
} step_motors[STEPS_MOTORS];

/* step motor ports&pins Init*/
void stepm_ports_init(const mx_pin_init_t* mx_port, GPIO_InitTypeDef* GPIO_InitStructure)
{
    while (mx_port->Port != NULL)
    {
        GPIO_ResetBits(mx_port->Port, mx_port->Pin);
        GPIO_InitStructure->GPIO_Pin = mx_port->Pin;
        GPIO_Init(mx_port->Port, GPIO_InitStructure);
        mx_port++;
    }
}

/* limit ports&pins init*/
void stepm_in_ports_init(const mx_pin_init_t* mx_port, GPIO_InitTypeDef* GPIO_InitStructure)
{
    while (mx_port->Port != NULL)
    {
        GPIO_InitStructure->GPIO_Pin = mx_port->Pin;
        GPIO_Init(mx_port->Port, GPIO_InitStructure);
        mx_port++;
    }
}

/* limit ports&pins init*/
void stepm_OC_init(const mx_pin_init_t* mx_port, GPIO_InitTypeDef* GPIO_InitStructure)
{
    while (mx_port->Port != NULL)
    {
        GPIO_InitStructure->GPIO_Pin = mx_port->Pin;
        GPIO_Init(mx_port->Port, GPIO_InitStructure);
        mx_port++;
    }
}

/* init step motor sw&hw*/
void stepm_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;

    steps_buf_count = 0;
    steps_buf_get = steps_buf_put = 0;

    NVIC_InitTypeDef NVIC_InitStructure;
    TIM_TimeBaseInitTypeDef TIM_TimeBase;

    mx_enable = 0;
    for (int i = 0; i < STEPS_MOTORS; i++)
    {
        step_motors[i].steps = 0;
        step_motors[i].clk = true;
        step_motors[i].isInProc = false;
        step_motors[i].globalSteps = 0;
    }
    //set enable dir steps pin as output
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    stepm_ports_init(mx_enables, &GPIO_InitStructure);
    stepm_ports_init(mx_dirs, &GPIO_InitStructure);
    stepm_ports_init(mx_steps, &GPIO_InitStructure);
    
    for (int i = 0; i < STEPS_MOTORS; i++)
    {
        GPIO_SetBits(mx_enables[i].Port, mx_enables[i].Pin);      //去使能该轴
    }

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;//init the limit pin as input pin
    stepm_in_ports_init(mx_limits, &GPIO_InitStructure);

    TIM_TimeBase.TIM_Prescaler = 1799;  // any
    TIM_TimeBase.TIM_Period = 100;      // any
    TIM_TimeBase.TIM_ClockDivision = 0;
    TIM_TimeBase.TIM_CounterMode = TIM_CounterMode_Up;

    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);

    const mx_timer_init_t* mx_timer = mx_timers;
    while (mx_timer->Timer != NULL)
    {
        NVIC_InitStructure.NVIC_IRQChannel = mx_timer->IRQn;
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
        NVIC_Init(&NVIC_InitStructure);

        RCC_APB1PeriphClockCmd(mx_timer->Clock, ENABLE);
        TIM_TimeBaseInit(mx_timer->Timer, &TIM_TimeBase);
        mx_timer->Timer->EGR = TIM_PSCReloadMode_Update;
        TIM_ClearITPendingBit(mx_timer->Timer, TIM_IT_Update);
        TIM_ARRPreloadConfig(mx_timer->Timer, ENABLE);
        TIM_ITConfig(mx_timer->Timer, TIM_IT_Update, ENABLE);


        mx_timer++;
    }
    /* 定时器输出通道1模式配置 */
    /* 模式配置：PWM模式1 */
//    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;
//    /* 输出状态设置：使能输出 */
//    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
//    /* 设置跳变值，当计数器计数到这个值时，电平发生跳变 */
////    TIM_OCInitStructure.TIM_Pulse = prescaler[0];
//    /* 当定时器计数值小于CCR1_Val时为高电平 */
//    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
//    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
//    TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
//    
//      TIM_OCInitStructure.TIM_Pulse = 999;

//    TIM_OC2Init(M0_TIM, &TIM_OCInitStructure);
//    TIM_OC2Init(M1_TIM, &TIM_OCInitStructure);
//    TIM_OC2Init(M2_TIM, &TIM_OCInitStructure);
//    TIM_OC2Init(M3_TIM, &TIM_OCInitStructure);
//  TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Disable);

}

//开始执行下一个动作
//从stepbuff中取一条指令并执行
void stepm_nextMove(void)
{
    // 1.检查是否所有轴都执行完了
    int i;
    for (i = 0; i < STEPS_MOTORS; i++)
    {
        if (step_motors[i].isInProc)
        {
            return;
        }
    }

    // Check for moves presents in steps buffer and all motors stops
    while (steps_buf_count != 0)
    {
        // Load next move
        __disable_irq();
        LINE_DATA* p = (LINE_DATA*)(&steps_buf[steps_buf_get]);//获取下一执行代码结构体
#if (USE_STEP_DEBUG == 1)
        memcpy(&cur_steps_buf, p, sizeof(cur_steps_buf)); // for debug
#endif

        mx_enable = 0;
        for (i = 0; i < STEPS_MOTORS; i++)
        {
            if (p->steps[i] != 0)
            {
                step_motors[i].steps = p->steps[i];
                step_motors[i].dir = p->dir[i];
                step_motors[i].clk = true;
                MX_STEP_OFF(mx_steps[i].Port, mx_steps[i].Pin);
                GPIO_WriteBit(mx_dirs[i].Port, mx_dirs[i].Pin, p->dir[i] ? Bit_SET : Bit_RESET);//设置方向

                GPIO_ResetBits(mx_enables[i].Port, mx_enables[i].Pin);      //使能该轴
                mx_timers[i].Timer->PSC = p->pscValue[i];                   //填充预分频
                TIM_SetAutoreload(mx_timers[i].Timer, p->arrValue[i]);      //更新arr
                step_motors[i].isInProc = true;
            }
        }

        steps_buf_get++;
        if (steps_buf_get >= STEPS_BUF_SIZE)
        {
            steps_buf_get = 0;
        }
        steps_buf_count--;

        for (i = 0; i < STEPS_MOTORS; i++)
        {
            if (step_motors[i].isInProc)
            {
                TIM_Cmd(mx_timers[i].Timer, ENABLE);
            }
        }
        __enable_irq();
        if (mx_enable != 0)
        {
            break;
        }
    }
}

//called at each timer interrupt，在定时器中断里面调用
void stepm_proc(uint8_t id)
{
    if (id >= STEPS_MOTORS)
    {
        return;
    }

    TIM_ClearITPendingBit(mx_timers[id].Timer, TIM_IT_Update);

    if (limits_chk())
    {
        stepm_EmergeStop();
        return;
    }

    if (step_motors[id].isInProc)
    {
        if (step_motors[id].clk)
        {
            MX_STEP_ON(mx_steps[id].Port, mx_steps[id].Pin);
        }
        else
        {
            MX_STEP_OFF(mx_steps[id].Port, mx_steps[id].Pin);
        }

        step_motors[id].clk = !step_motors[id].clk;
        if (!step_motors[id].clk)
        {
            if (step_motors[id].steps != 0)
            {
                step_motors[id].steps--;
            }
            if (step_motors[id].dir)
            {
                step_motors[id].globalSteps++;
            }
            else
            {
                step_motors[id].globalSteps--;
            }
        }
        else if (step_motors[id].steps == 0)
        {
            TIM_Cmd(mx_timers[id].Timer, DISABLE);
            step_motors[id].isInProc = false;
            stepm_nextMove();//每当有一个轴执行完后，检查时候可以执行下一个动作
        }
    }
}

void stepm_EmergeStop(void)
{
    __disable_irq();

    steps_buf_count = 0;
    steps_buf_get = steps_buf_put = 0;
    for (int i = 0; i < STEPS_MOTORS; i++)
    {
        step_motors[i].steps = 0;
        if (step_motors[i].clk)
        {
            TIM_Cmd(mx_timers[i].Timer, DISABLE);
            TIM_ClearITPendingBit(mx_timers[i].Timer, (TIM_IT_Update | TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4 | TIM_IT_COM | TIM_IT_Trigger | TIM_IT_Break));
            step_motors[i].isInProc = false;
            GPIO_SetBits(mx_enables[i].Port, mx_enables[i].Pin);
        }
    }
    __enable_irq();
}

//
//向stepbuff中加一条指令，由外部调用
void stepm_addMove(uint32_t steps[4], uint32_t frq[4], uint8_t dir[4])
{
    uint32_t pscValue;
    int i;

    bool empty = true;
    for (i = 0; i < STEPS_MOTORS; i++)
    {
        if (steps[i] != 0)
        {
            empty = false;
            break;
        }
    }
    if (empty)
    {
        return;
    }

    LINE_DATA* p = (LINE_DATA*)(&steps_buf[steps_buf_put]);

    while (steps_buf_count >= STEPS_BUF_SIZE)
    {
        // __WFI();
    }

    //f= 72M/((arr+1)*(psc+1))
    for (i = 0; i < STEPS_MOTORS; i++)
    {
        uint32_t f = frq[i];
        if (f > (15000 * K_FRQ))
        {
            f = 15000 * K_FRQ;  // 15kHz
        }
        if (f < K_FRQ)
        {
            f = K_FRQ;          // 1Hz
        }

        // SystemCoreClock / (psc * arr) = frq
        pscValue = 1;//初始不分频，当 arr>65535的时候再分频

        uint32_t arrValue = (SystemCoreClock / 2 * K_FRQ) / f; // (1 falling age on 2 IRQ)
        while ((arrValue & 0xffff0000) != 0)
        {
            pscValue = pscValue << 1;   //*2
            arrValue = arrValue >> 1;   // /2
        }
        pscValue--;
        p->f[i] = frq[i]; // for debug
        p->arrValue[i] = arrValue;
        p->pscValue[i] = pscValue;
        p->dir[i] = dir[i];
        p->steps[i] = steps[i];
    }

    steps_buf_put++;
    if (steps_buf_put >= STEPS_BUF_SIZE)
    {
        steps_buf_put = 0;
    }
    __disable_irq();
    steps_buf_count++;
    if (mx_enable == 0)
    {
        stepm_nextMove();
    }
    __enable_irq();

}

// 获取剩余缓冲区指令数
int32_t stepm_getRemainLines(void)
{
    return steps_buf_count;
}

// 步进电机是否有指令还未执行完
int32_t stepm_inProc(void)
{
    if (steps_buf_count > 0)
    {
        return true;
    }
    for (int i = 0; i < STEPS_MOTORS; i++)
    {
        if (step_motors[i].isInProc)
        {
            return true;
        }
    }
    return false;
}

// 指令缓冲区是否已满
uint32_t stepm_LinesBufferIsFull(void)
{
    return steps_buf_count >= STEPS_BUF_SIZE;
}

// 获取某轴的全局步数
int32_t stepm_getCurGlobalStepsNum(uint8_t id)
{
    return step_motors[id].globalSteps;
}

uint32_t stepm_getRemSteps(uint8_t id)
{
    return step_motors[id].steps;
}

// 清除所有轴的全局步数
void stepm_ZeroGlobalCrd(void)
{
    for (int i = 0; i < STEPS_MOTORS; i++)
    {
        step_motors[i].globalSteps = 0;
    }
}


//show linedata
void step_dump()
{
    LINE_DATA* p = (LINE_DATA*)(&cur_steps_buf);
    for (int i = 0; i < 4; i++)
    {
        scr_gotoxy(1, 7 + i);
        scr_printf("%d,%d,%d,%d [%d]   ", p->steps[i], p->dir[i], p->pscValue[i], p->arrValue[i], p->f[i]); // TODO
    }
}
